/**
* Author: Ha Dang (email: hris2003@gmail.com)
*
* \file moveEmotion.cpp
* \brief Move NAO's body to express emotions.
*
*/
#include "moveEmotion.h"
#ifdef __linux__
  // Linux Includes Here
  #include <sys/time.h>
  timeval start;
#elif _WIN32
  // Windows Includes Here
  #include <ctime>
  time_t start;
#endif

#ifdef __linux__
  // Linux Includes Here
timeval getTime(){
    timeval a;
    //timeval b;

    gettimeofday(&a, 0);
    return a;
}
#elif _WIN32
  // Windows Includes Here
time_t getTime(){
    time_t a;
    //timeval b;

    time(&a);
    return a;
}
#endif

#ifdef __linux__
  // Linux Includes Here
float getDifftime(timeval m_start){

    return getTime().tv_sec - m_start.tv_sec;
}
#elif _WIN32
  // Windows Includes Here
float getDifftime(time_t m_start){

    return (float) difftime( getTime(), m_start);
}
#endif

float moveEmotion::genRand(float maxFloat){

      /* generate secret number between 0 and maxFloat: */
      return (float)(((rand() % 10) / 10.0) * maxFloat);
}

/*
  *NAO's head (H25, V4.0) can move between:
  * Pitch: 0.5149 (forward) to -0.672 (backward)
  * Yaw  : 2.0857 (leftside) to -2.0857 (rightside)
  *
  * Rules to map between (Valence, Arousal) and (HeadPitch, HeadYaw):
  *  - Valence positive: pitch backward (i.e. gaze up) and small yaw angle variation
  *  - Valence negative: pitch forward (i.e. gaze down) and big yaw angle variation
  *
  *  - Arousal  : inversely mapped with execution time of each designated head's movement,
  *  generally, the faster the movement is, the higher the arousal is.
  *
  *@param:
  *valence compris between -1 and 1
  * arousal compris between 0 and 1
  */
int moveEmotion::moveAffectiveHead(float valence, float arousal){

    HeadPitch = valence / -2.0;
    HeadTime = 6*(1 - arousal);//time between 1->6
    //HeadYaw = (1-valence)*2.0;


    if (valence < 0){
        HeadYaw = genRand(2) - 1.0;
    }else{
        HeadYaw = genRand(1.0) - 0.5;
    }
    //std::cout<<"(valence,HeadPitch):("<<valence<<","<<HeadPitch<<")"<<std::endl;
    return int(HeadTime);
}

/* NAO's hand can be completely open (1) to completely closed (0).
  *
  * Mapping rules between (valence, arousal) to nao's hand movement:
  *  - Valence : the greater the valence is, the more open the hand is.
  *
  *  - Arousal : inversely mapped with execution time of each designated hand's movement,
  *  generally, the faster the movement is, the higher the arousal is.
  *
  *
  *valence compris between -1 and 1
  * arousal compris between 0 and 1
  */
int moveEmotion::moveAffectiveHand(float valence, float arousal){

    RHand = LHand = (valence + 1) / 2.0;
    HandTime = 2.5-arousal;//time between 0.5->2.5
    return int(HandTime);
}



/*
  *nao's LShoulder (H25, V4.0) can move between:
  * Pitch : 2.0857 (back) to -2.0857 (front)
  * Roll  : 1.3265 (outward) to -0.3142 (inward)
  *
  * Mapping rule for LShoulder's movement and (valence, arousal):
  *  - Valence (-1, 1) => (1.5, 0) LShoulderPitch : the greater the valence is, the higher the arm is.
  *  - Arousal (0, 1)  => (-0.15,1.15) LShoulderYaw: the greater the arousal, the more ouverture the left arm makes.
  *  - Arousal (0,)  => execution time: the higher the arousal is, the faster the movement is.
  *
  *nao's RShoulder (H25, V4.0) can move between:
  * Pitch : 2.0857 (back) to -2.0857 (front)
  * Roll  : -1.3265 (outward) to 0.3142 (inward)
  *
  * Mapping rule for RShoulder's movement and (valence, arousal)
  *  - Valence (-1, 1) => (1.5, 0) RShoulderPitch : the greater the valence is, the higher the arm is.
  *  - Arousal (0, 1)  => (0.15, -1.5) RShoulderYaw : the greater the arousal is, the more ouverture the right arm makes
  *  - Arousal (0,1)  => execution time: the higher the arousal is, the faster the movement is.
  *
  */

int moveEmotion::moveAffectiveShoulder(float valence, float arousal){
    LShoulderRoll = arousal + (genRand(0.3) - 0.15);
    LShoulderPitch = 0.7*(1.25-valence) + (genRand(0.1) - 0.05);
    LShoulderTime = 2*(2.5 - arousal);//0.5->3.5 seconds

    RShoulderRoll = -arousal+ (genRand(0.3) - 0.15);
    RShoulderPitch = 0.7*(1.25-valence) + (genRand(0.1) - 0.05);
    RShoulderTime = LShoulderTime;//1.5->3.5 seconds
    return LShoulderTime;
}




/*
  *
  * Execute the motion in speed portion mode, better to map between Arousal and Speed.
  *
  *
  */
int moveEmotion::moveRobotWithSpeed(AL::ALMotionProxy motion, float speedPortion){


    //Setup joints' names and angles
    AL::ALValue jointsName;
    AL::ALValue targetAngles;
    if (desiredJoints & 1){
        jointsName.arrayPush("HeadYaw");jointsName.arrayPush("HeadPitch");
        targetAngles.arrayPush(HeadYaw);targetAngles.arrayPush(HeadPitch);
    }

    if (desiredJoints & 2){
        jointsName.arrayPush("LShoulderRoll");jointsName.arrayPush("LShoulderPitch");
        jointsName.arrayPush("RShoulderRoll");jointsName.arrayPush("RShoulderPitch");
        targetAngles.arrayPush(LShoulderRoll);targetAngles.arrayPush(LShoulderPitch);
        targetAngles.arrayPush(RShoulderRoll);targetAngles.arrayPush(RShoulderPitch);
    }

    if (desiredJoints & 4){
        jointsName.arrayPush("LHand");jointsName.arrayPush("RHand");
        targetAngles.arrayPush(LHand);targetAngles.arrayPush(RHand);
    }



    try{

        motion.angleInterpolationWithSpeed(jointsName, targetAngles, speedPortion);

    }catch (const AL::ALError& e) {
      std::cerr << "Caught exception: " << e.what() << std::endl;
      return 1;
    }
    return 0;
}

//===============================================================
/*
  * For debug only.
  *
  * Sadness  : valence = (-1, -0.5), arousal = (0, 0.25), speedPortion = 0.05
  *
  * Happiness: ++head, hand++ : valence = (0.3, 0.6), arousal = (0.3, 1.0), speedPortion = 0.12
  *            ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.12
  *
  * Anger: ++head, hand++ : valence = (-1.0, 1.0), arousal = (0.3, 1.0), speedPortion = 0.6
  *        ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.6
  *
  * Serenity: ++head++             : valence = (-0.3, 0.7), arousal = (0.0, 0.25), speedPortion = 0.1
  *           ++hand, shoulder++   : valence = (-0.5, 0.5), arousal = (0.0, 0.25), speedPortion = 0.1
  *
  */
//===============================================================
/*
  * Intensity is between (0, 1). The higher the intensity, the sadder the expression should be.
  * This can be expressed by the HeadPitch, higher Headpitch is higher intensity; and in motion speed,
  * the higher the intensity, the slower the motion is.
  */
void moveEmotion::moveSadness(AL::ALMotionProxy motion, float intensity,float maxTime)
{

    //SpeedPortion should be between (0.05, 0.1)
    //std::cout<<"Sad head"<<std::endl;
	if (intensity < 0){
		intensity = 0;
	}
	if (intensity > 1.0){
		intensity = 1.0;
	}
    float speedPortion = 0.05 - intensity/50.0;
    float t = 0;
    start = getTime();
    while (t < maxTime){
        moveAffectiveHead(-0.5+genRand(-intensity), genRand(0.25));
        moveAffectiveHand(-0.5+genRand(-0.5), genRand(0.25));
        moveAffectiveShoulder(-0.5+genRand(-0.5), genRand(0.25));
        moveRobotWithSpeed(motion, speedPortion);
        t = getDifftime(start);
        //std::cout<<t<<std::endl;

    }
}

/*
  * Happiness: ++head, hand++ : valence = (0.3, 0.6), arousal = (0.3, 1.0), speedPortion = 0.12
  *            ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.12
  *            intensity affects 30% of the speed of the motion. The higher the intensity, the faster the motion is.
  *
*/
void moveEmotion::moveHappiness(AL::ALMotionProxy motion,float intensity, float maxTime){
    //std::cout<<"Happy head"<<std::endl;
	if (intensity < 0){
		intensity = 0;
	}
	if (intensity > 1.0){
		intensity = 1.0;
	}

    float speedPortion = 0.1 + intensity/30.0;
    float t = 0;
    start = getTime();
    while (t < maxTime){
        moveAffectiveHead(0.3+genRand(0.3), 0.3+genRand(0.7));
        moveAffectiveHand(0.3+genRand(0.3), 0.3+genRand(0.7));
        moveAffectiveShoulder(genRand(0.9) - 0.4, 0.3+genRand(0.7));
        moveRobotWithSpeed(motion, speedPortion);
        t = getDifftime(start);
        //std::cout<<t<<std::endl;
    }
}

void moveEmotion::moveAnger(AL::ALMotionProxy motion, float intensity, float maxTime){
    //std::cout<<"Anger head"<<std::endl;
	if (intensity < 0){
		intensity = 0;
	}
	if (intensity > 1.0){
		intensity = 1.0;
	}
    float t = 0;
    float speedPortion = 0.4 + genRand(intensity) * 0.3;
    start = getTime();
    while (t < maxTime){
        moveAffectiveHead(-1.0+genRand(2.0), 0.7+genRand(0.3));
        moveAffectiveHand(-1.0+genRand(2.0), 0.7+genRand(0.3));
        moveAffectiveShoulder(-1.0+genRand(2.0), 0.3+genRand(0.7));
        moveRobotWithSpeed(motion, speedPortion);
        t = getDifftime(start);
        //std::cout<<t<<std::endl;
    }
}

void moveEmotion::moveSerenity(AL::ALMotionProxy motion,float intensity, float maxTime){
	if (intensity < 0){
		intensity = 0;
	}
	if (intensity > 1.0){
		intensity = 1.0;
	}
    float t = 0;
    float speedPortion = 0.1 - genRand(intensity) * 0.07;
    start = getTime();
    while (t < maxTime){
        moveAffectiveHead(genRand(1.0)-0.3, genRand(0.25));
        moveAffectiveHand( genRand(1.0)-0.5, genRand(0.25));
        moveAffectiveShoulder( genRand(1.0)-0.7, genRand(0.25));
        moveRobotWithSpeed(motion, speedPortion);
        t = getDifftime(start);
        //std::cout<<"Time: "<<t<<std::endl;

    }
}

/*
 * From rightmost to leftmost: bit 1: head, bit 2: shoulder, bit 3: hand
 * For example: joint = 1 means head only
 *              joint = 2 means shoulder only
 *              joint = 3 means head + shoulder
 *              joint = 4 means hand only
 *              joint = 5 means head + hand
 *              joint = 6 means shoulder + hand
 *              joint = 7 means haed + shoulder + hand
 */
int moveEmotion::setDesiredJoints(int joint){
    if ((joint == 0)|| (joint > 7)){
        std::cout<<"Desired joints invalid. Valid joint value is between [1, 7]. Stop!"<<std::endl;
        return 1;
    }
    desiredJoints = joint;
	return 0;
};
